#ifndef NAITOLIB_H
#define NAITOLIB_H

#include <tsubamegaeshi-base/action.h>
#include <math.h>

// $B3F4X@a>pJs(B
/*
llj1 $B:88T4X@a$N2sE>(B($B30B&J}8~$,(B+)
rlj1 $B1&8T4X@a$N2sE>(B($BFbB&J}8~$,(B+)
llj2 $B:88T4X@a$N6~?-(B($B6J$2$kJ}8~$,(B+)
rlj2 $B1&8T4X@a$N6~?-(B($B6J$2$kJ}8~$,(B+)
llj3 $B:88T4X@a$N3+5S(B($B3+$/J}8~$,(B+)
rlj3 $B1&8T4X@a$N3+5S(B($BJD$8$kJ}8~$,(B+)
llj4 $B:8I($N6~?-(B($B?-$P$9J}8~$,(B+)
rlj4 $B1&I($N6~?-(B($B?-$P$9J}8~$,(B+)
llj5 $B:8B-<s$N=D2sE>(B($B6J$2$kJ}8~$,(B+)
rlj5 $B1&B-<s$N=D2sE>(B($B6J$2$kJ}8~$,(B+)
llj6 $B:8B-<s$N2#2sE>(B($B30B&J}8~$,(B+)
rlj6 $B1&B-<s$N2#2sE>(B($BFbB&J}8~$,(B+)
laj1 $B:88*$N=D2sE>(B($BA0J}8~$,(B+)
raj1 $B1&8*$N=D2sE>(B($BA0J}8~$,(B+)
laj2 $B:88*$N2#2sE>(B($B30B&J}8~$,(B+)
raj2 $B1&8*$N2#2sE>(B($BFbB&J}8~$,(B+)
laj3 $B:88*2<$N=D2sE>(B($B30B&J}8~$,(B+)
raj3 $B1&8*2<$N=D2sE>(B($BFbB&J}8~$,(B+)
laj4 $B:8I*$N6~?-!J6J$2$kJ}8~$,(B+$B!K(B
raj4 $B1&I*$N6~?-!J6J$2$kJ}8~$,(B+$B!K(B
*/


class Naitolib :public Action
{
 public:
    Naitolib(Worldmodel::shared_ptr world) : Action(world) { }
    ~Naitolib() {};
    
 protected:
    typedef std::map<std::string, double> Hinge;
    Hinge cur_joint; //$B8=:_$N3QEY(B
    Hinge new_joint; //$B<!$N3QB.EY(B
    Hinge aim_joint; //$BL\E*$N3QEY(B

 public:
 
    // $B8=:_$N3QEY$N<hF@$HL\I83QEY$N@_Dj(B
    void init() { 
	Worldmodel::TStatus& wst = world_->current_status();
	// $B>uBV$r=PNO(B
	wst.dump();

	new_joint.clear();

	Worldmodel::THJSenceMap::iterator hi = wst.hj_.begin();
	while (hi != wst.hj_.end()) {
	    cur_joint[hi->first] = hi->second;
	    set(hi->first, aim_joint[hi->first]);
	    hi++;
	}

	Worldmodel::TUJSenceMap::iterator ui = wst.uj_.begin();
	while (ui != wst.uj_.end()) {
	    std::string uj1st,uj2nd;
	    uj1st = uj2nd = ui->first;
	    uj1st.erase(4, 2);
	    uj2nd.erase(3, 2);
	    cur_joint[uj1st] = ui->second.first;
	    cur_joint[uj2nd] = ui->second.second;
	    set(uj1st, aim_joint[uj1st]);
	    set(uj2nd, aim_joint[uj2nd]);
	    ui++;
	}
    }

    // $B3F4X@a$N3QB.EY$NAw?.(B
    void setangle() {
	Hinge::iterator i = new_joint.begin();

	while (i != new_joint.end()) {
// hj
	    if        (i->first == "raj3") {
		rae3(i->second);
	    } else if (i->first == "raj4") {
		rae4(i->second);
	    } else if (i->first == "laj3") {
		lae3(i->second);
	    } else if (i->first == "laj4") {
		lae4(i->second);
	    } else if (i->first == "rlj1") {
		rle1(i->second);
	    } else if (i->first == "rlj4") {
		rle4(i->second);
	    } else if (i->first == "llj1") {
		lle1(i->second);
	    } else if (i->first == "llj4") {
		lle4(i->second);
//uj
	    } else if (i->first == "raj1" ||
		       i->first == "raj2") {
		rae1_2(new_joint["raj1"], new_joint["raj2"]);
	    } else if (i->first == "laj1" ||
		       i->first == "laj2") {
		lae1_2(new_joint["laj1"], new_joint["laj2"]);
	    } else if (i->first == "rlj2" ||
		       i->first == "rlj3") {
		rle2_3(new_joint["rlj2"], new_joint["rlj3"]);
	    } else if (i->first == "llj2" ||
		       i->first == "llj3") {
		lle2_3(new_joint["llj2"], new_joint["llj3"]);
	    } else if (i->first == "rlj5" ||
		       i->first == "rlj6") {
		rle5_6(new_joint["rlj5"], new_joint["rlj6"]);
	    } else if (i->first == "llj5" ||
		       i->first == "llj6") {
		lle5_6(new_joint["llj5"], new_joint["llj6"]);
	    } else {
		std::cerr << "Error!! set_function" << std::endl;
	    }
	    i++;
	}
    }

    // $B4X@a$N3QEY$N@_Dj(B
    bool set(std::string id, double angle, double gain = 0.05 ) {

	const double EPS = 0.2;
	
	// $B3QEY$N:9$rD4$Y$F(BEPS$B$h$jBg$-$1$l$PF0$+$9(B
	if (fabs(cur_joint[id] - angle) > EPS) {
	    new_joint[id] = gain * (angle - cur_joint[id]);
	    aim_joint[id] = angle;
	    
	    //std::cout << "move(" << id << ", " << angle << ") "
	    //	      << cur_joint[id] << " " << new_joint[id] << std::endl;
	    return true;
	}
	new_joint[id] = 0;
	return false;
    }

     // $B4X@a$N3QEY$N<hF@(B
     double get(std::string id) {
	 return cur_joint[id];
     }

};

#endif//NAITOLIB_H
